/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-07-17     MBW       the first version
 */
#ifndef CONTROL_SINS_H_
#define CONTROL_SINS_H_

#include <sys.h>
#include "math.h"
#include <stdio.h>
#include "maths.h"
#include "filter.h"

#define SINS_T 0.005f
#define high_delay  2

#define pos_delay_num  5

#define TIME_CONTANST_ZER    0.5f  //0.08
#define K_ACC_ZER           (1.0f / (TIME_CONTANST_ZER * TIME_CONTANST_ZER * TIME_CONTANST_ZER)) //1
#define K_VEL_ZER           (3.0f / (TIME_CONTANST_ZER * TIME_CONTANST_ZER))                        //2
#define K_POS_ZER           (3.0f / TIME_CONTANST_ZER)                                            //2

#define TIME_CONTANST_ZER2   0.8f
#define K_ACC_ZER2           (1.0f / (TIME_CONTANST_ZER2 * TIME_CONTANST_ZER2 * TIME_CONTANST_ZER2))
#define K_VEL_ZER2           (3.0f / (TIME_CONTANST_ZER2 * TIME_CONTANST_ZER2))
#define K_POS_ZER2           (3.0f / TIME_CONTANST_ZER2)

typedef struct
{
    F_XYZ state_delta;

    F_XYZ last_acc;
    F_XYZ acc;
    F_XYZ acc_cor;

    F_XYZ vel;
    F_XYZ vel_org;
    F_XYZ vel_cor;
    F_XYZ vel_delta;

    F_XYZ pos;
    F_XYZ pos_org;
    F_XYZ pos_cor;
    F_XYZ pos_history[pos_delay_num];

    U16_XYZ pos_delay_cnt;


    uint16_t high_start_cnt;
}_SINS;

float sins_high(float aacz , float height );
extern _SINS sins;

#endif /* CONTROL_SINS_H_ */
